import cv2
import serial
import smbus2
import json
import time
import math

class I2C_servo:    
    def __init__(self, I2C_IP, I2C_BUS):
        self.bus = smbus2.SMBus(I2C_BUS)
        self.I2C_IP = I2C_IP
        self.pitch = 0
        self.yaw = 0
    
    def I2C_send_json_data(self, pitch, yaw, delay = 0.02, PR = False):
        self.pitch = pitch
        self.yaw = yaw

        data = json.dumps({"pitch": self.pitch, "yaw": self.yaw}).encode('utf-8') 
        if PR == True:
            print(data)

        time.sleep(delay)
        try:
            self.bus.write_byte(self.I2C_IP, ord("R"))  
            for byte in data:
                self.bus.write_byte(self.I2C_IP, byte)  

            self.bus.write_byte(self.I2C_IP, ord("S"))

        except OSError as e:
            print("Failed to send byte:", e)

class I2C_ESP_RS:
    def __init__(self, I2C_IP, I2C_BUS):
        self.I2C_IP = I2C_IP
        self.bus = I2C_BUS
        self.str_data_s = ""
        self.str_data_r = ""

    def I2C_write_str(self, str_data_s, delay = 0.02):
        self.str_data_s = str_data_s.encode('utf-8')

        time.sleep(delay)
        try:
            for byte in self.str_data_s:
                self.bus.write_byte(self.I2C_IP, byte)  

        except OSError as e:
            print("Failed to send byte:", e)


    def I2C_read_str(self, newline = False, long = 32, delay = 0.02):
        try:
            data = self.bus.read_i2c_block_data(self.I2C_IP, 0, long)
            string_data = ''.join(chr(byte) for byte in data if long <= byte <= 126)
            if newline == True:
                string_data = string_data.replace("\n", "")
            # print(f"Received data: {string_data}")
            time.sleep(delay)

            return string_data
            
        except OSError as e:
            print("Failed to send byte:", e)

class Serial:
    def __init__(self, COM_ID, band, timeout):
        self.ser = serial.Serial(COM_ID, band, timeout = timeout)
        # response = self.ser.readline().decode().strip()
        # print("Received confirmation message:", response)
        # self.serial_send = True
        self.pitch_i = 0
        self.yaw = 0

    def Serial_send_json(self, yaw_i, pitch_i):
        self.yaw_i = yaw_i
        self.pitch_i = pitch_i
        
        # 
        #  True:
        
        try:
            data = {
                "pitch:": self.pitch_i,
                "yaw:": self.yaw_i
            }
            json_str = json.dumps(data)

            self.ser.write(json_str.encode())
            print("JSON data sent:", json_str.encode())
            # 等待接收确认消息
            response = self.ser.readline().decode().strip()
            print("Received confirmation message:", response)

        except Exception as e:
            print("Error sending JSON data:", str(e))



class servo:
    def __init__(self, kp_x = 0.5, ki_x = 0.01, kd_x = 0.4, kp_y = 0.5, ki_y = 0.01, kd_y = 0.4, servo_x_analg = math.radians(120), servo_y_analg = math.radians(90)):
        self.kp_x = kp_x
        self.kd_x = kd_x

        self.ki_x = ki_x
        self.ki_y = ki_y

        self.kp_y = kp_y
        self.kd_y = kd_y

        self.error_cx=0
        self.error_cy=0

        self.x_analg_integral = 0
        self.y_analg_integral = 0

        self.last_error_x_analg_differential = 0
        self.last_error_y_analg_differential = 0

        self.servo_x_analg = servo_x_analg
        self.servo_y_analg = servo_y_analg

        self.yaw_i = 0
        self.pitch_i = 0
        self.serial_send = True

    def map_value_yaw(self, error_cx, img_width_max = 640, servo_x_analg = math.radians(90)):
        analg = (error_cx / img_width_max) * servo_x_analg
        return round(analg, 4)

    def map_value_pitch(self, error_cy, img_height_max = 480, servo_y_analg = math.radians(60)):
        analg = (error_cy / img_height_max) * servo_y_analg
        return round(analg, 4)

    def servo_run(self, img, box, tracking):

        if tracking == True:
            
            height, width = img.shape[:2]
            
            x, y, w, h = box

            cx = int(x + w / 2)
            cy = int(y + h / 2)

            # PID中P环
            self.error_cx = width / 2 - cx
            self.error_cy = height / 2 - cy

            # PID中I环
            self.x_analg_integral += self.error_cx
            self.y_analg_integral += self.error_cy

            # PID中PD环运算
            self.x_analg = self.kp_x * self.error_cx + self.ki_x * self.x_analg_integral + self.kd_x * (self.error_cx - self.last_error_x_analg_differential)
            self.y_analg = self.kp_y * self.error_cy + self.ki_y * self.y_analg_integral + self.kd_y * (self.error_cy - self.last_error_y_analg_differential)

            # PID中D环
            self.last_error_x_analg_differential = self.error_cx
            self.last_error_y_analg_differential = self.error_cy

            self.error_x_analg = self.map_value_yaw(self.x_analg, img_width_max = width, servo_x_analg = self.servo_x_analg)
            self.error_y_analg = self.map_value_pitch(self.y_analg, img_height_max = height, servo_y_analg = self.servo_y_analg)

            # print("error_x_analg:", self.error_x_analg,"error_y_analg:", self.error_y_analg)
            # print(f"Updated PID parameters X: Kp={self.kp_x}, Ki={self.ki_x}, Kd={self.kd_x}")
            # print(f"Updated PID parameters Y: Kp={self.kp_y}, Ki={self.ki_y}, Kd={self.kd_y}")
            
            if self.error_x_analg > self.servo_x_analg/2:
                self.error_x_analg = self.servo_x_analg/2

            if self.error_x_analg < -self.servo_x_analg/2:
                self.error_x_analg = -self.servo_x_analg/2

            if self.error_y_analg > self.error_y_analg/2:
                self.error_y_analg = self.error_y_analg/2

            if self.error_y_analg < -self.error_y_analg/2:
                self.error_y_analg = -self.error_y_analg/2


            self.yaw_i = -self.error_x_analg
            self.pitch_i = self.error_y_analg
            self.serial_send = True
                    


################################################## 
#                 调试闭环PID控制                 #
################################################## 
class debug_mode():
    def __init__(self, kp_x = 0.5, ki_x = 0.01, kd_x = 0.4, kp_y = 0.5, ki_y = 0.01, kd_y = 0.4):
        self.kp_x_cs = kp_x  # 假设的初始值，应该根据实际情况设置
        self.ki_x_cs = ki_x # 假设的初始值
        self.kd_x_cs = kd_x  # 假设的初始值
        self.kp_y_cs = kp_y     # 假设的初始值
        self.ki_y_cs = ki_y    # 假设的初始值
        self.kd_y_cs = kd_y     # 假设的初始值

    def debug_main(self):
        window_name_1 = 'PID Control Yaw'
        window_name_2 = 'PID Control Pitch'

        cv2.namedWindow(window_name_1)
        cv2.createTrackbar('Kp_x', window_name_1, int(self.kp_x_cs * 100), 100, self.update_pid_params_x)  # 使用 self.update_pid_params_x 替换 nothing
        cv2.createTrackbar('Ki_x', window_name_1, int(self.ki_x_cs * 100), 100, self.update_pid_params_x)
        cv2.createTrackbar('Kd_x', window_name_1, int(self.kd_x_cs * 100), 100, self.update_pid_params_x)
        
        cv2.namedWindow(window_name_2)
        cv2.createTrackbar('Kp_y', window_name_2, int(self.kp_y_cs * 100), 100, self.update_pid_params_y)  # 使用 self.update_pid_params_y 替换 nothing
        cv2.createTrackbar('Ki_y', window_name_2, int(self.ki_y_cs * 100), 100, self.update_pid_params_y)
        cv2.createTrackbar('Kd_y', window_name_2, int(self.kd_y_cs * 100), 100, self.update_pid_params_y)

        while True:
            self.update_pid_params_x(1)
            self.update_pid_params_y(1)

            key = cv2.waitKey(1) & 0xFF
            if key == 27:
                break


    def update_pid_params_x(self, x):
        # 假设每个滑动条控制一个PID参数
        self.kp_x_cs = cv2.getTrackbarPos('Kp_x', 'PID Control Yaw') / 100.0
        self.ki_x_cs = cv2.getTrackbarPos('Ki_x', 'PID Control Yaw') / 100.0
        self.kd_x_cs = cv2.getTrackbarPos('Kd_x', 'PID Control Yaw') / 100.0
        kp_x, ki_x, kd_x = self.kp_x_cs, self.ki_x_cs, self.kd_x_cs
        # print(f"Updated PID parameters X: Kp={self.kp_x_cs}, Ki={self.ki_x_cs}, Kd={self.kd_x_cs}")

    def update_pid_params_y(self, y):
        # 假设每个滑动条控制一个PID参数
        self.kp_y_cs = cv2.getTrackbarPos('Kp_y', 'PID Control Pitch') / 100.0
        self.ki_y_cs = cv2.getTrackbarPos('Ki_y', 'PID Control Pitch') / 100.0
        self.kd_y_cs = cv2.getTrackbarPos('Kd_y', 'PID Control Pitch') / 100.0
        kp_y, ki_y, kd_y = self.kp_y_cs, self.ki_y_cs, self.kd_y_cs
        # print(f"Updated PID parameters Y: Kp={self.kp_y_cs}, Ki={self.ki_y_cs}, Kd={self.kd_y_cs}")
